#!/usr/bin/env python
# Copyright (c) 2016 The UUV Simulator Authors.
# All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""**Description**

Node to call the ROS service from the plume server to load
the plume particles from an YAML file.

**Input ROS parameters**

* `filename` (*type:* `string`): Input YAML file where the plume particles' coordinates and time of creation are stored

**ROS launch snippet**

```xml
<node name="load_plume_particles"
      pkg="uuv_plume_simulator"
      type="load_plume_particles"
      output="screen">
    <rosparam>
        filename: /<path>/file.yaml
    </rosparam>
</node>
```

"""
import rospy
import sys
from uuv_plume_msgs.srv import LoadPlumeParticles


if __name__ == '__main__':
    rospy.init_node('load_plume_particles')
    rospy.loginfo('Load plume particles from file')

    if rospy.is_shutdown():
        rospy.logerr('ROS master not running!')
        sys.exit(-1)

    if rospy.has_param('~filename'):
        filename = rospy.get_param('~filename')
    else:
        raise rospy.ROSException('No filename found')

    try:
        rospy.wait_for_service('load_plume_particles', timeout=30)
    except rospy.ROSException:
        raise rospy.ROSException('Service not available! Closing node...')

    try:
        load_particles = rospy.ServiceProxy(
            'load_plume_particles',
            LoadPlumeParticles)
    except rospy.ServiceException as e:
        raise rospy.ROSException('Service call failed, error=' + e)

    success = load_particles(filename)

    if success:
        rospy.loginfo('Plume successfully loaded, filename=%s', filename)
    else:
        rospy.loginfo('Failed to load particles')
